#include "prox_sensors.h"

#include "lpc17xx_pinsel.h"
#include "lpc17xx_adc.h"
#include "lpc_types.h"
#include "lpc17xx_libcfg_default.h"
#include "lpc17xx_gpio.h"

/* Private flags ---------------------------------------------------------- */
uint8_t proximity_sensors_init = 0;


/* Private global variables ----------------------------------------------- */


/* Init functions --------------------------------------------------------- */

void init_proximity_sensors(void)
{
    if (proximity_sensors_init)
        return;
        
    ADC_Init(LPC_ADC, 50000);
    
    PINSEL_CFG_Type PinCfg;
    PinCfg.OpenDrain   = PINSEL_PINMODE_NORMAL;
    PinCfg.Pinmode     = PINSEL_PINMODE_PULLUP;
    
    // Left front sensor
    PinCfg.Funcnum     = LF_FUNC;
    PinCfg.Portnum     = LF_PORT;
    PinCfg.Pinnum      = LF_PIN;
    PINSEL_ConfigPin(&PinCfg);
    ADC_ChannelCmd(LPC_ADC, LF_CHAN, ENABLE);/* */
    
    // Right front sensor
    PinCfg.Funcnum     = RF_FUNC;
    PinCfg.Portnum     = RF_PORT;
    PinCfg.Pinnum      = RF_PIN;
    PINSEL_ConfigPin(&PinCfg);
    ADC_ChannelCmd(LPC_ADC, RF_CHAN, ENABLE);/* */
    
    // Right rear sensor
    PinCfg.Funcnum     = RR_FUNC;
    PinCfg.Portnum     = RR_PORT;
    PinCfg.Pinnum      = RR_PIN;
    PINSEL_ConfigPin(&PinCfg);
    ADC_ChannelCmd(LPC_ADC, RR_CHAN, ENABLE);/* */
    
    // Left rear sensor
    PinCfg.Funcnum     = LR_FUNC;
    PinCfg.Portnum     = LR_PORT;
    PinCfg.Pinnum      = LR_PIN;
    PINSEL_ConfigPin(&PinCfg);
    ADC_ChannelCmd(LPC_ADC, LR_CHAN, ENABLE);/* */
    
    ADC_BurstCmd(LPC_ADC, ENABLE);
    
    // Front digital sensor
    GPIO_SetDir(0, FRONT_SENS, 0);
    
    proximity_sensors_init = 1;
    
    return;
}

/* Sensor read functions -------------------------------------------------- */
uint8_t sens_to_chan(PROX_SENSOR_SELECTION sens)
{
    switch (sens)
    {
        case LF_SENSOR :
            return LF_CHAN;
        case RF_SENSOR :
            return RF_CHAN;
        case RR_SENSOR :
            return RR_CHAN;
        case LR_SENSOR :
            return LR_CHAN;
        default :
            return 0;
    }
}

uint16_t read_sensor_value(PROX_SENSOR_SELECTION sens)
{
    if (!proximity_sensors_init)
        return 1;
        
    CHECK_PARAM(PARAM_PROX_SENSORS_SEL(sens));
    
    return ADC_ChannelGetData(LPC_ADC, sens_to_chan(sens));
}


inline
float pow(float res, float a, int b)
{
    return (b == 0 ? res : pow(res * a, a, b - 1));
}

uint8_t read_sensor_distance(PROX_SENSOR_SELECTION sens)
{
    if (!proximity_sensors_init)
        return 0;
        
    CHECK_PARAM(PARAM_PROX_SENSORS_SEL(sens));
    
    float result = 0;
    float reading = (float)(ADC_ChannelGetData(LPC_ADC, sens_to_chan(sens)) +
                     ADC_ChannelGetData(LPC_ADC, sens_to_chan(sens)) +
                     ADC_ChannelGetData(LPC_ADC, sens_to_chan(sens)) +
                     ADC_ChannelGetData(LPC_ADC, sens_to_chan(sens)) +
                     ADC_ChannelGetData(LPC_ADC, sens_to_chan(sens))) / 5;
                     
    switch(sens)
    {
        case (LF_SENSOR):
                result = - 2.33239e-18 * pow(1, reading, 6)
                         + 2.58878e-14 * pow(1, reading, 5)
                         - 1.11159e-10 * pow(1, reading, 4)
                         + 2.26677e-7  * pow(1, reading, 3)
                         - 2.03369e-4  * pow(1, reading, 2)
                         + 0.0174495   * reading
                         + 80.2193;
            break;
        case (RF_SENSOR) :
                result = + 1.31613e-18 * pow(1, reading, 6)
                         - 1.79627e-14 * pow(1, reading, 5)
                         + 9.71958e-11 * pow(1, reading, 4)
                         - 2.67516e-7  * pow(1, reading, 3)
                         + 4.00934e-4  * pow(1, reading, 2)
                         - 0.330855    * reading
                         + 149.702;
            break;
        case (RR_SENSOR) :
                result = + 1.14168e-18 * pow(1, reading, 6)
                         - 1.30963e-14 * pow(1, reading, 5)
                         + 6.10401e-11 * pow(1, reading, 4)
                         - 1.51990e-7  * pow(1, reading, 3)
                         + 2.27564e-4  * pow(1, reading, 2)
                         - 0.220058    * reading
                         + 130.455;
            break;
        case (LR_SENSOR) :
                result = + 3.59603e-18 * pow(1, reading, 6)
                         - 4.22235e-14 * pow(1, reading, 5)
                         + 2.00553e-10 * pow(1, reading, 4)
                         - 4.95993e-7  * pow(1, reading, 3)
                         + 6.85856e-4  * pow(1, reading, 2)
                         - 0.529509    * reading
                         + 209.147;
            break;
        default :
            return 0;
    }
    
    return (uint8_t)((result > 70 ? 70 : (result < 8 ? 8 : result)));
}

uint8_t read_front_sensor()
{
    return (GPIO_ReadValue(0) & FRONT_SENS ? 1 : 0);
}